#include <Eigen/Dense>

#include <igl/barycenter.h>
#include <igl/doublearea.h>
#include <igl/gaussian_curvature.h>
#include <igl/invert_diag.h>
#include <igl/massmatrix.h>
#include <igl/cotmatrix.h>
#include <igl/writeOBJ.h>

#include <iostream>
#include <vector>
#include "particle2realflowUtil/slVector.H"
#include "particle2realflowUtil/kdTree.H"
#include "myFunctions.h"


using namespace Eigen;
using namespace std;

// Compute centroid and subtract, scale it to uniformed scale
void meshScale(Eigen::MatrixXd & Vertices, Eigen::MatrixXi & Faces) {
	VectorXd dblA_V;
	igl::doublearea(Vertices, Faces, dblA_V);
	double area_V = 0.5*dblA_V.sum();
	MatrixXd BC_V;
	igl::barycenter(Vertices, Faces, BC_V);
	RowVector3d centroid_V(0, 0, 0);
	for (int i = 0; i<BC_V.rows(); i++)
	{
		centroid_V += 0.5*dblA_V(i) / area_V * BC_V.row(i);
	}
	Vertices.rowwise() -= centroid_V;
	// Normalize to unit surface area (important for numerics)
	Vertices.array() /= sqrt(area_V);
	std::cout << "The centroid dispacement of uniformed model is: " << centroid_V << std::endl;
	std::cout << "The double area of uniformed model is: " << area_V << std::endl;
}


//  Compute integral of Gaussian curvature
void getGussianCurvature(const Eigen::MatrixXd & Vertices, const Eigen::MatrixXi & Faces, 
	Eigen::VectorXd &GussianCurvature) 
{
	igl::gaussian_curvature(Vertices, Faces, GussianCurvature);
	SparseMatrix<double> gussianM, gussianMinv;
	igl::massmatrix(Vertices, Faces, igl::MASSMATRIX_TYPE_DEFAULT, gussianM);
	igl::invert_diag(gussianM, gussianMinv);
	GussianCurvature = (gussianMinv*GussianCurvature).eval();
	std::cout << "The max Gussian curvature is: " << GussianCurvature.maxCoeff() << endl;
	std::cout << "The min Gussian curvature is: " << GussianCurvature.minCoeff() << endl;
}


//  Compute integral of Laplacian Matrix
void getLaplacianMatrix(const Eigen::MatrixXd & Vertices, const Eigen::MatrixXi & Faces, 
	Eigen::SparseMatrix<double> & LaplaceMatrix) 
{
	igl::cotmatrix(Vertices, Faces, LaplaceMatrix);
	SparseMatrix<double> LapM, LapMinv;
	igl::massmatrix(Vertices, Faces, igl::MASSMATRIX_TYPE_VORONOI, LapM);

	// Invert the diagonal entries of mass matrix
	igl::invert_diag(LapM, LapMinv);
	LaplaceMatrix = (LapMinv * (LaplaceMatrix.eval()) );
}
/*
// calculate the offset of each vertices of the mesh
void calculateMeshOffset(const Eigen::MatrixXd & Vertices, const std::vector<SlVector3> & Particles, 
						const Eigen::MatrixXd & ParticleOffset, KDTree & particleKDTree, int num, double radius, 
						Eigen::MatrixXd & verticeOffset) 
{
	std::vector<int> neighbors;
	for (int i = 0; i < Vertices.rows(); ++i) {
		neighbors.clear();
		SlVector3 currentPoint = SlVector3( Vertices(i,0), Vertices(i, 1), Vertices(i, 2) );
		particleKDTree.neighbors(Particles, currentPoint, num, radius, neighbors);
		if (neighbors.size() > 0) {
			Eigen::Vector3d offset(0.0);
			for (int j = 0; j < neighbors.size(); ++j) {
				offset += ParticleOffset.row( neighbors.at(j) );
			}
			offset = offset / ((double)(neighbors.size()) );
			verticeOffset.row(i) = offset;
		}
		else {
			verticeOffset.row(i) = 0.001 * Eigen::Vector3d::Random();
		}

	}
}
*/

// calculate singularities of each vertices
// radius represents for the maximum searching area
// num represents for maximum number of particles which we take into account
void calculateMeshAnisotropy(const Eigen::MatrixXd & Vertices, const std::vector<SlVector3> & Particles, 
	KDTree & particleKDTree,int num, double radius, const Eigen::MatrixXd & ParticleAnisotropy,
	Eigen::MatrixXd & VerticesAnisotropy, Eigen::VectorXi &numPointsOfPerVer)
{
	std::vector<int> neighborsIndex;
	for (int i = 0; i < Vertices.rows(); ++i) {
		neighborsIndex.clear();
		SlVector3 currentVertices = SlVector3(Vertices(i, 0), Vertices(i, 1), Vertices(i, 2));
		particleKDTree.neighbors(Particles, currentVertices, num, radius, neighborsIndex);

		numPointsOfPerVer(i) = neighborsIndex.size();
		if (neighborsIndex.size() >= LeastAffectiveParticles) {	//we believe it won't work well if there is few particles
			
			//compute weight
			Eigen::VectorXd weight( neighborsIndex.size() );
			for (int j = 0; j < neighborsIndex.size(); ++j) {
				SlVector3 slPoint = Particles.at(neighborsIndex.at(j));
				double distance = mag(slPoint - currentVertices);

				double cubicDistWeight = pow(distance / radius, 3);
				if (cubicDistWeight > 1.0) cubicDistWeight = 1.0;

				weight(j)=(1.0 - cubicDistWeight);
			}

			//compute sum of weight
			double sumWeight = weight.sum();
			
			//compute delta singular value: maxSingular - minSingular 
			Eigen::VectorXd anisotropyVector(neighborsIndex.size());
			for (int j = 0; j < neighborsIndex.size(); ++j) {
				Eigen::VectorXd singularValue = ParticleAnisotropy.row( neighborsIndex.at(j) );
				anisotropyVector(j) = (singularValue(0)*2.0 - singularValue(1) - singularValue(2) );
			}
			VerticesAnisotropy(i,0) = ( weight.dot(anisotropyVector) )/sumWeight ;
		}
		else {
			VerticesAnisotropy(i, 0) = 0.0;
		}

	}

	std::cout << "Maximum number of points belongs to vertices: "
		<< numPointsOfPerVer.maxCoeff() << endl;
	std::cout << "Minimum number of points belongs to vertices: "
		<< numPointsOfPerVer.minCoeff() << endl;
	std::cout << "Average number of points belongs to vertices: "
						<< (double)(numPointsOfPerVer.sum()) / (double)(numPointsOfPerVer.size()) << endl;

	cout << "Maximun vertices singular value is: " << VerticesAnisotropy.col(0).maxCoeff() << endl;
	cout << "Minimun vertices singular value is: " << VerticesAnisotropy.col(0).minCoeff() << endl;
	cout << "Average vertices singular value is: " << 
		VerticesAnisotropy.col(0).sum() / (double)(VerticesAnisotropy.col(0).size()) << endl;

	std::cout << "Vertices Anisotropy computation accomplished!" << endl;
}

